/**
 *  sem.c - Semaphore Mechanism.
 *
 *  Copyright (C) 2008-2010 ZhangHu
 *  All rights reserved.
 *  E-MAIL: anmnmnly@gmail.com
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include "include/sem.h"


//static list_t sem_head; // pointer to the head and rear of semaphore queue



/**
 * initsem - Initialize sem_head.
 * @return:
 *
 * @notes:
 */
void initsem(void)
{
//    sem_head.next = &sem_head;
//    sem_head.prev = &sem_head;
}



/**
 * check_id - Check the ID is whether existed.
 * @semid: The semaphore ID that need to check out.
 * @return: Semaphore or NULL.
 *
 * @notes: If the ID has existed in semaphore queue, and then return
 *         semaphore's address, otherwise return NULL.
 */
/*(static sem_t *check_id(semid_t semid)
{
    sem_t *psem = NULL;
    list_t *plist = NULL;

    if((sem_head.next == &sem_head) ||
        (sem_head.prev == &sem_head))
    {
        return(NULL);
    }

    plist = sem_head.next;
    do {
        psem = mac_find_entry(plist, sem_t, sem_link);
        if(semid == psem->sem_id) {
            return(psem);
        } else {
            plist = plist->next;
        }
    } while(plist != sem_head.next);

    return(NULL);
}*/



/**
 * rdy_to_sem_wait - Move TCB from ready queue to semaphore waiting queue.
 * @psem: Pointer to semaphore.
 * @ptcb: Pointer to TCB.
 * @return:
 *
 * @notes:
 */
static void rdy_to_sem_wait(sem_t *psem, tcb_t *ptcb)
{
    uint32 prio = 0;
    list_t *del_tcb = NULL;

    if((psem == NULL) || (ptcb == NULL)) {
        return;
    }

    prio = ptcb->prio;
    del_tcb = del_tcb_rdy(ptcb);
    if(del_tcb != NULL) {
        add_node_list_rear(&psem->task_link[prio], del_tcb);
    }
}



/**
 * sem_wait_to_rdy - Move TCB form semaphore waiting queue to ready queue
 * @psem: Pointer to semaphore.
 * @del_tcb: Pointer to TCB.
 * @return:
 *
 * @notes:
 */
static void sem_wait_to_rdy(sem_t *psem, list_t *del_tcb)
{
    list_t *plist = NULL;
    tcb_t *ptcb = NULL;

    if((psem == NULL) || (del_tcb == NULL)) {
        return;
    }

    ptcb = mac_find_entry(del_tcb, tcb_t, link);
    plist = del_node_list(&psem->task_link[ptcb->prio], del_tcb);
    ptcb = mac_find_entry(plist, tcb_t, link);
    add_tcb_rdy(ptcb);
}



/**
 * find_max_prio_tcb - Find the highest priority task in semaphore waiting queue
 *                     and return the link_address.
 * @psem: Pointer to semaphore.
 * @return: The 'link' member of TCB. otherwise return NULL.
 *
 * @notes:
 */
static list_t *find_max_prio_tcb(sem_t *psem)
{
    uint32 i = 0;

    if(psem == NULL) {
        return(NULL);
    }

    for(i = 0; i < MAX_PRIO; i++) {
        if(psem->task_link[i].next !=  &psem->task_link[i]) {
            return psem->task_link[i].next;
        }
    }

    return(NULL);
}




/* The following functions are application program interface */
/**
 * semCreate - Creating a semaphore.
 * @InitCnt: The initial count value of new semaphore.
 * @return: The entry of new semaphore.
 *
 * @notes:
 */
sem_t *semCreate(sem_t *pSemaphore, uint32 InitCnt)
{
    uint32 i = 0;

    if(pSemaphore == NULL) {
        return(NULL);
    }

    pSemaphore->sem_link.next = &pSemaphore->sem_link;
    pSemaphore->sem_link.prev = &pSemaphore->sem_link;
    pSemaphore->sem_cnt = InitCnt;
    pSemaphore->task_cnt = 0;
    pSemaphore->tcb_link.next = NULL;

    for(i = 0; i < MAX_PRIO; i++) {
        pSemaphore->task_link[i].next = &pSemaphore->task_link[i];
        pSemaphore->task_link[i].prev = &pSemaphore->task_link[i];
    }

    return(pSemaphore);
}



/**
 * semInit - Creating a semaphore.
 * @InitCnt: The initial count value of new semaphore.
 * @return: The entry of new semaphore.
 *
 * @notes:
 */
sem_t *semInit(uint32 InitCnt)
{
    sem_t *psem = NULL;

    psem = (sem_t*)kmalloc(sizeof(sem_t));

    if(psem == NULL) {
        return(NULL);
    }

    return(semCreate(psem, InitCnt));
}



/**
 * semInitMutex - Creating a mutex semaphore.
 * @return: The entry of new semaphore, otherwise NULL for failed.
 *
 * @notes:
 */
sem_t *semInitMutex(void)
{
    return(semInit(1));
}



/**
 * semGain - Get semaphore, if operating is failed,
 *           the task will be block, and its TCB will be
 *           deleted from ready queue, and then insert it
 *           into waiting queue of semaphore.
 * @pSem: Pointer to semaphore.
 * @return:
 *
 * @notes: SemID or pSemaphore is valide to specify a semaphore.
 */
void semGain(sem_t *pSem)
{
    mac_disable_irq();

    // semaphore is available
    if(pSem->sem_cnt > 0) {
        pSem->sem_cnt--;
        mac_enable_irq();

        add_node_single_list_head(&(current()->sem_link),
                                  &pSem->tcb_link);
        return;
    }

    // semaphore is unavailable, so current task is blocked.
    pSem->task_cnt++;
    rdy_to_sem_wait(pSem, current());
    mac_enable_irq();

    current()->status = T_BLOCK;
    scheduler();
}



/**
 * semGainFree - Get semaphore, if the operating is failed,
 *               the task should not be blocked.
 * @pSem: Pointer to semaphore.
 * @return:
 *
 * @notes:
 */
bool_t semGainFree(sem_t *pSem)
{

    mac_disable_irq();

    if(pSem->sem_cnt > 0) {
        pSem->sem_cnt--;
        mac_enable_irq();

        add_node_single_list_head(&(current()->sem_link), &pSem->tcb_link);
        return TRUE;
    }

    mac_enable_irq();
    return FALSE;
}



/**
 * semPost - Unlock a semaphore.
 * @pSem: Pointer to semaphore.
 * @return: Return results about this operation.
 *
 * @notes:
 */
void semPost(sem_t *pSem)
{
    sem_t *sem = NULL;
    list_t *plist = NULL;
    slist_t *slist = NULL;
    tcb_t *ptcb = NULL;

    slist = &(current()->sem_link);
    slist = slist->next;
    while(slist != NULL) {
        sem = mac_find_entry(slist, sem_t, tcb_link);
        if((uint32)sem == (uint32)pSem) {
            del_node_single_list(&(current()->sem_link), slist);
            break;
        }
        slist = slist->next;
    }

    mac_disable_irq();

    pSem->sem_cnt++;
    if(pSem->sem_cnt <= 0) {
        mac_enable_irq();
        return;
    }

    plist = find_max_prio_tcb(pSem);
    if(plist == NULL) {
        mac_enable_irq();
        return;
    }

    pSem->sem_cnt--;
    pSem->task_cnt--;
    mac_enable_irq();

    ptcb = mac_find_entry(plist, tcb_t, link);
    add_node_single_list_head(&ptcb->sem_link, &pSem->tcb_link);
    ptcb->status = T_READY;

    mac_disable_irq();
    sem_wait_to_rdy(pSem, plist);
    mac_enable_irq();

    scheduler();
    return;
}



/**
 * semDel - Deleting a semaphore.
 * @pSem: Pointer to semaphore.
 * @return: Return results of this operation.
 *
 * @notes:
 */
void semDel(sem_t *pSem)
{
    uint32 i = 0;
    sem_t *sem = NULL;
    list_t *plist = NULL;
    slist_t *slist = NULL;
    tcb_t *ptcb = NULL;

    if(pSem == NULL) {
        return;
    }

    mac_disable_irq();

    // if current task gained this semophore.
    slist = &(current()->sem_link);
    slist = slist->next;
    while(slist != NULL) {
        sem = mac_find_entry(slist, sem_t, tcb_link);
        if((uint32)sem == (uint32)pSem) {
            del_node_single_list(&(current()->sem_link), slist);
            break;
        }
        slist = slist->next;
    }

    // move all tasks in waiting queue to sleep queue.
    for(i=0; i<MAX_PRIO; i++) {
        do {
            plist = pSem->task_link[i].next;
            if(plist == (&pSem->task_link[i])) {
                break;
            } else {
                del_node_list(&pSem->task_link[i], plist);
                ptcb = mac_find_entry(plist, tcb_t, link);
                ptcb->status = T_SLEEP;
                add_tcb_sleep(ptcb);
            }
        } while(1);
    }

    mac_enable_irq();
    kfree(pSem);

    return;
}

